#include "step_algorithm.h"
#include <stdio.h>
float distance = 0;
//xy坐标俯仰角相对于坐标原点的解算寄存
float angle_syn_x=0;
float angle_syn_y=0;

//单片机浮点数开方运算
float SquareRootFloat(float number)
{//STM32-72M主频耗时8us
	long i;
	float x, y;
	const float f = 1.5F;
	
	x = number * 0.5F;
	y = number;
	i = * ( long * ) &y;
	i = 0x5f3759df - ( i >> 1 );
	y = * ( float * ) &i;
	y = y * ( f - ( x * y * y ) );
	y = y * ( f - ( x * y * y ) );
	return number * y;
}

//单位为米
//设置距离
void set_distance(float distance_set)
{
    distance = distance_set;
}

//原点单轴解算——x
float angle_calculation_x(float x)
{
    float r_tan = 0;
    float r_pi = 0;
    float r_angle = -1;//在前面的代表符号
    
    if(x < 0 )
    {
        x= (-1*x);
        r_angle = -1*r_angle;
    }
    r_tan = 0.001f*x/distance;
    r_pi = atanf(r_tan);
    r_angle = r_angle*r_pi/PI*180.0f;
    return r_angle;
}

//横轴运动,在distance设置正确时，误差较小，可用
void step_distance_ctrl_x_alone(float x,STEP_MOTOR*motor)
{
    //将距离转化为角度
    float angle = angle_calculation_x(x);
    step_struct_setangle(motor,angle);
    step_struct_setsnF(motor,0);//设置为单独运动，不多机协同
    step_angle_ctrl(motor);
}
//原点单轴解算——y
float angle_calculation_y(float y)
{
    float r_tan = 0;
    float r_pi = 0;
    float r_angle = 1;//在前面的代表符号
    
    if(y < 0 )
    {
        y= (-1*y);
        r_angle = -1*r_angle;
    }
    r_tan = 0.001f*y/distance;
    r_pi = atanf(r_tan);
    r_angle = r_angle*r_pi/PI*180.0f;
    return r_angle;
}

void step_distance_ctrl_y_alone(float y,STEP_MOTOR*motor)
{
    //将距离转化为角度
    float angle = angle_calculation_y(y);
    step_struct_setangle(motor,angle);
    step_struct_setsnF(motor,0);//设置为单独运动，不多机协同
    step_angle_ctrl(motor);
}
//x-mm,y-mm
void angle_calculation_xy(float x,float y)
{
    float r_tan_x = 0;
    float r_pi_x = 0;
    float r_angle_x = -1;//在前面的代表符号

    float r_tan_y = 0;
    float r_pi_y = 0;
    float r_angle_y = 1;//在前面的代表符号
    float r_distance_y = 0;

    if(x < 0 )
    {
        x= (-1*x);
        r_angle_x = -1*r_angle_x;
    }
    r_tan_x = 0.001f*x/distance;
    r_pi_x = atanf(r_tan_x);
    r_angle_x = r_angle_x*r_pi_x/PI*180.0f;

    if(y < 0 )
    {
        y= (-1*y);
        r_angle_y = -1*r_angle_y;
    }
    r_distance_y = (distance*distance)+(0.001f*x*0.001f*x);
    r_distance_y = SquareRootFloat(r_distance_y);
    r_tan_y = 0.001f*y/r_distance_y;
    r_pi_y = atanf(r_tan_y);
    r_angle_y = r_angle_y*r_pi_y/PI*180.0f;

    angle_syn_x = r_angle_x;
    angle_syn_y = r_angle_y;
}

//多机同步控制坐标距离——ok
void multiple_step_distance_ctrl(float x,float y)
{
    angle_calculation_xy(x,y);//更新两个电机角度
    step_struct_setangle(&motor1,angle_syn_x);
    step_struct_setangle(&motor2,angle_syn_y); 
    step_struct_setsnF(&motor1,1);
    step_struct_setsnF(&motor2,1);
    multiple_step_syn_angle_ctrl(arr,2);
}

// 直线插补：计算两个电机的角度更新，使其沿直线从(x1, y1)运动到(x2, y2)
void linear_interpolation(float x1, float y1, float x2, float y2, float step_size)
{
    // 计算起点到终点的直线距离
    float distance_x = x2 - x1;
    float distance_y = y2 - y1;
    float total_distance = SquareRootFloat(distance_x * distance_x + distance_y * distance_y);

    // 计算每个步进的步长
    float steps = total_distance / step_size;

    // 计算每次插补的x和y增量
    float delta_x = distance_x / steps;
    float delta_y = distance_y / steps;

    // 当前坐标
    float current_x = x1;
    float current_y = y1;

    // 每次更新步进电机的位置
    for (int i = 0; i < steps; i++)
    {
        // 更新当前坐标
        current_x += delta_x;
        current_y += delta_y;

        // 计算当前坐标对应的电机角度
        angle_calculation_xy(current_x*1000.0f, current_y*1000.0f);// 将米转换为毫米

        // 设置步进电机的目标角度
        step_struct_setangle(&motor1, angle_syn_x);
        step_struct_setangle(&motor2, angle_syn_y);

        // 设置同步控制标志
        step_struct_setsnF(&motor1, 1);
        step_struct_setsnF(&motor2, 1);

        // 执行同步角度控制
        multiple_step_syn_angle_ctrl(arr, 2);

        // 这里可以加入延时，控制步伐的平滑度（如果需要）
    }

    // 最后一次角度控制，确保精确到达终点
    angle_calculation_xy(x2*1000.0f, y2*1000.0f);// 将米转换为毫米
    step_struct_setangle(&motor1, angle_syn_x);
    step_struct_setangle(&motor2, angle_syn_y);
    step_struct_setsnF(&motor1, 1);
    step_struct_setsnF(&motor2, 1);
    multiple_step_syn_angle_ctrl(arr, 2);
}
